/*
 *  MSR206.cpp
 *  msr206-lib
 *
 *  Created by Chris Moos on 8/5/07.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "MSR206.h"

#include <stdio.h>
#include <sys/types.h>
#include <fcntl.h>
#include <termios.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <ctype.h>
#include <sys/ioctl.h>
#include <iostream>

//#define DEBUG

using namespace std;

static inline uint8_t swap_bits(uint8_t n) {
  n = ((n&0xF0) >>4 ) | ( (n&0x0F) <<4);
  n = ((n&0xCC) >>2 ) | ( (n&0x33) <<2);
  n = ((n&0xAA) >>1 ) | ( (n&0x55) <<1);

  return  n;
};

MSR206::MSR206()
{
	fd = -1;
}
/*-------------------------------------------------------------------------------------*/
MSR206::~MSR206()
{

}
/*-------------------------------------------------------------------------------------*/
bool MSR206::isConnected()
{
	if(this->fd > 0) return true;
	return false;
}
/*-------------------------------------------------------------------------------------*/
void MSR206::init()
{
	if(!isConnected()) throw "Unable to initialize: Not connected to device.";
	
	this->sendReset();	
	if(!this->commTest()) throw "Unable to initialize: Communications Test failed.";
	this->sendReset();
}
/*-------------------------------------------------------------------------------------*/
int MSR206::write_bytes(char *buf, int num)
{
	int ret = 0;
	
	if(!isConnected()) throw "Unable to send data: Not connected to device.";
	ret = write(this->fd, buf, num);
	if(ret <= 0) throw "Unable to send data: write() failed";

	#ifdef DEBUG
	printf("Sent Data(%d bytes): ", num);
	for(int x = 0; x < num; x++) printf("[%02x] ", buf[x]);
	printf("\n");
	#endif
	
	return ret;
}
/*-------------------------------------------------------------------------------------*/
int MSR206::read_bytes(unsigned char *buf, int num)
{
	int temp = 0;
	int ret;
	
	if(buf == NULL) return -1;
	
	while(temp != num) {
		ret = read(this->fd, buf+temp, num - temp);
		if(ret < 0) return -1;
		if(ret > 0) temp += ret;
	}
	
	#ifdef DEBUG
	printf("Received Data(%d bytes): ", num);
	for(int x = 0; x < num; x++) printf("[%02x] ", buf[x]);
	printf("\n");
	#endif
	
	return num;
}
/*-------------------------------------------------------------------------------------*/
void MSR206::readTrack(unsigned char * &outBuf, unsigned int &outLen, char trackOptions)
{
	unsigned char buf[1024];
	unsigned int len;
	
	/* track header */
	read_bytes((unsigned char*)&buf, 2);
	if(memcmp(buf, "\x1b", 1) != 0) {
		throw "Unable to read data: Expected Track Header";
	}
	
	/* track length */
	if(read_bytes((unsigned char*)&buf, 1) != 1) {
		throw "Unable to read data: Expected Track Length";
	}
	if(buf[0] > 254) {
		throw "Unable to read data: Invalid length received";
	}
	else if(buf[0] == 0) { /* no data for this track */
		outBuf = NULL;
		outLen = 0;
		return;
	}
	
	len = buf[0];

	/* track data */
	read_bytes((unsigned char*)&buf, len);

    if(trackOptions == TRACK_7BIT) decode((unsigned char*)&buf, len, outBuf, outLen, TRACK_7BIT);
	if(trackOptions == TRACK_5BIT) decode((unsigned char*)&buf, len, outBuf, outLen, TRACK_5BIT);

//	if(trackOptions == TRACK_7BIT) decode_7bit((unsigned char*)&buf, len, outBuf, outLen);
//	if(trackOptions == TRACK_5BIT) decode_5bit((unsigned char*)&buf, len, outBuf, outLen);
	
}
/*-------------------------------------------------------------------------------------*/
void MSR206::free_ms_data(magnetic_stripe_t *ms_data)
{
	if(ms_data == NULL) return;
	
	if(ms_data->track1 != NULL) free(ms_data->track1);
	if(ms_data->track2 != NULL) free(ms_data->track2);
	if(ms_data->track3 != NULL) free(ms_data->track3);
	
	free(ms_data);
}
/*-------------------------------------------------------------------------------------*/
magnetic_stripe_t *MSR206::readCard(char track1_format, char track2_format, char track3_format)
{
	unsigned char buf[1024];
	unsigned char end[3];
	magnetic_stripe_t *ms_data = (magnetic_stripe_t*)malloc(sizeof(magnetic_stripe_t));
	
	if(!isConnected()) {
		free_ms_data(ms_data);
		throw "Unable to read card: Not connected to device.";
	}
	
	/* set track format */
	this->setBPC(track1_format, track2_format, track3_format);
	
	write_bytes(MSR_READ_RAW, 2);
	
	/* check for ack */
	read_bytes((unsigned char*)&buf, 2);
    if(memcmp(buf, MSR_READ_ACK, 2) != 0) {
		free_ms_data(ms_data);
		throw "Unable to read data: Invalid Response";
	}
    
	readTrack(ms_data->track1, ms_data->t1_len, track1_format);
	readTrack(ms_data->track2, ms_data->t2_len, track2_format);
	readTrack(ms_data->track3, ms_data->t3_len, track3_format);
	
	/* read end */
	read_bytes((unsigned char*)&end, 3);
	if(memcmp(end, MSR_END_READ, 3) != 0) {
		free_ms_data(ms_data);
		throw "Unable to read data: Invalid end response";
	}
	
	/* read status byte */
	read_bytes((unsigned char*)&end, 1);
	if(end[0] == MSR_STATUS_OK) {
		return ms_data;
	}
	else { /* error messages */
		free_ms_data(ms_data);
		switch(end[0])
		{
			case MSR_STATUS_WRITE_READ_ERROR:
				throw "Unable to read card: Write or read error";
				
			case MSR_STATUS_COMMAND_FORMAT_ERROR:
				throw "Unable to read card: Command format error";
				
			case MSR_STATUS_INVALID_COMMAND:
				throw "Unable to read card: Invalid command";
				
			case MSR_STATUS_INVALID_SWIPE_WRITE_MODE:
				throw "Unable to read card: Invalid card swipe when in write mode";
				
			default:
				throw "Unable to read card: Unknown error code";
		}
	}
	
	return ms_data;
}
/*-------------------------------------------------------------------------------------*/
void MSR206::getLeadingZeros(leading_zeros_t *zeros)
{
	unsigned char buf[3];
	
	if(!isConnected()) throw "Unable to check leading zeros: Not connected to device.";
	if(zeros == NULL) throw "Invalid leading zeros structure passed";
	
	write_bytes(MSR_CHECK_ZEROS, 2);
	
	if(read_bytes((unsigned char*)&buf, 3) != 3) throw "Unable to check leading zeros: invalid response";
	if(memcmp(buf, "\x1b", 1) != 0) throw "Unable to check leading zeros: bad resposne";
	
	zeros->t1t3 = buf[1];
	zeros->t2 = buf[2];
}
/*-------------------------------------------------------------------------------------*/

int MSR206::decode(unsigned char *inBuf, unsigned int inLen, unsigned char * &outBuf, unsigned int &outLen, int bpc)
{
	unsigned int bytes = inLen;
	outBuf = (unsigned char*)malloc(bytes);
    char byte = 0;
    outLen = bytes;
        
    for (int i = 0; i < inLen; i++) {
        byte = inBuf[i];
        
        // Strip the parity bit
        byte &= ~1;
        
        if (bpc < 7) {
            byte = swap_bits(byte);
            byte /= 8;
            byte |= 0x30;
        } else {
            byte = swap_bits(byte);
            byte /= 2;
            if (byte < 0x20)
                byte |= 0x20;
            else {
                byte |= 0x40;
                byte -= 0x20;
            }
        }
        outBuf[i] = byte;
        if (byte == '?') {
            outLen=i+1;
            break;
        }
    }

    //print_bits(outBuf, outlen);
    //print_bytes(outbuf, outlen);

    return 0;
}


void MSR206::decode_7bit(unsigned char *buf, unsigned int len, unsigned char * &outBuf, unsigned int &outLen)
{
	unsigned int bytes = (len * 8) / 7;
	outBuf = (unsigned char*)malloc(bytes);
	unsigned int tempLen = 0;
	char *test;
	
	test = (char*)outBuf;
	
	for(int y = 0; y < (len/7); y++) {
        
        if(*buf == 0x00 || buf[1] == 0x00) break;
        
        test[0] = swap_bits(buf[0] & 0xfc) + 0x20;
        test[1] = swap_bits(((buf[1] & 0xf8) >> 1) | ((buf[0] & 1) << 7)) + 0x20;
        test[2] = swap_bits(((buf[2] & 0xf0) >> 2) | ((buf[1] & 3) << 6)) + 0x20; 
        test[3] = swap_bits(((buf[3] & 0xe0) >> 3) | ((buf[2] & 7) << 5)) + 0x20;
        test[4] = swap_bits(((buf[4] & 0xc0) >> 4) | ((buf[3] & 0xf) << 4)) + 0x20;
        test[5] = swap_bits(((buf[5] & 0x80) >> 5) | ((buf[4] & 0x1f) << 3)) + 0x20;
        test[6] = swap_bits((buf[5] & 0x3f) << 2) + 0x20;
        test[7] = swap_bits((buf[6] & 0x7e) << 1) + 0x20;
        
        tempLen += 8;
        buf += 7;
        test += 8;
        
	}
	
	outLen = tempLen;
}
/*-------------------------------------------------------------------------------------*/
void MSR206::decode_5bit(unsigned char *buf, unsigned int len, unsigned char * &outBuf, unsigned int &outLen)
{
	unsigned int bytes = (len * 8) / 5;
	outBuf = (unsigned char*)malloc(bytes);
	unsigned int tempLen = 0;
	char *test;
	
	test = (char*)outBuf;
	
	for(int y = 0; y < (len/5); y++) {
	
	if(*buf == 0x00 || buf[1] == 0x00) break;
	
	test[0] = swap_bits(buf[0] & 0xf8) + 0x30;
	test[1] = swap_bits(((buf[1] & 0x80) >> 3) | ((buf[0] & 7) << 5)) + 0x30;
	test[2] = swap_bits((buf[1] & 0x3c) << 2) + 0x30; 
	test[3] = swap_bits(((buf[2] & 0xe0) >> 1) | ((buf[1] & 1) << 7)) + 0x30;
	test[4] = swap_bits((buf[2] & 0xf) << 4) + 0x30;
	test[5] = swap_bits((buf[3] & 0x78) << 1) + 0x30;
	test[6] = swap_bits((buf[4] & 0xc0) >> 2 | ((buf[3] & 3) << 6)) + 0x30;
	test[7] = swap_bits((buf[4] & 0x1e) << 3) + 0x30;
	
	tempLen += 8;
	buf += 5;
	test += 8;
	
	}
	
	outLen = tempLen;
}
/*-------------------------------------------------------------------------------------*/
void MSR206::disconnect()
{
	if(!isConnected()) throw "Unable to close connection: not connected";
	sendReset();
	sendReset();
	sendReset();
	close(this->fd);
}
/*-------------------------------------------------------------------------------------*/
void MSR206::print_bytes(unsigned char *bytes, int len)
{
	printf("Bytes: ");
	for(int x = 0; x < len; x++) printf("%02x ", bytes[x]);
	printf("\n");
}

void MSR206::print_bits(unsigned char *bytes, int len)
{
    int byte, i;
    for (byte = 0; byte < len; byte++) {        
        for (i = 7; i > -1; i--) {
            if (bytes[byte] & (1 << i))
                printf("1");
            else
                printf("0");
        }
    }
    printf ("\n");
}


/*-------------------------------------------------------------------------------------*/
bool MSR206::setBPC(char track1, char track2, char track3)
{
	char ack[5];
	char bpc_str[6];
	
	if(!isConnected()) throw "Unable to set bpc: not connected";
	
	memcpy(bpc_str, (void*)MSR_SET_BPC, 2);
	memcpy(&bpc_str[2], (void*)&track1, 1);
	memcpy(&bpc_str[3], (void*)&track2, 1);
	memcpy(&bpc_str[4], (void*)&track3, 1);
	
	
	write_bytes(bpc_str, 5);
	
	if(read_bytes((unsigned char*)&ack, 5) != 5) throw "Unable to set BPC: invalid response";
	if(memcmp(ack, MSR_SET_BPC_ACK, 5) == 0) return true;
	
	return false;
}
/*-------------------------------------------------------------------------------------*/
bool MSR206::commTest()
{
	char ack[2];
	
	if(!isConnected()) throw "Unable to perform comm test: not connected";
	
	write_bytes(MSR_COMM_TEST, 2);
		
	if(read_bytes((unsigned char*)&ack, 2) != 2) throw "Comm test failed: invalid response";
	
	if(memcmp(ack, MSR_COMM_TEST_ACK, 2) == 0) return true;
	
	return false;
}
/*-------------------------------------------------------------------------------------*/
void MSR206::setRedLEDOn()
{
	if(!isConnected()) throw "Unable to turn Red LED on: not connected";
	write_bytes(MSR_RED_LED_ON, 2);
}
/*-------------------------------------------------------------------------------------*/
void MSR206::setAllLEDOn()
{
	if(!isConnected()) throw "Unable to turn all LEDs on: not connected";
	write_bytes(MSR_ALL_LIGHTS_ON, 2);
}
/*-------------------------------------------------------------------------------------*/
void MSR206::setAllLEDOff()
{
	if(!isConnected()) throw "Unable to turn all LEDs off: not connected";
	write_bytes(MSR_ALL_LIGHTS_OFF, 2);
}
/*-------------------------------------------------------------------------------------*/
void MSR206::sendReset()
{
	if(!isConnected()) throw "Unable to send reset: not connected";
	
	write_bytes(MSR_RESET, 2);
}

unsigned char MSR206::getModel()
{
    msr_model_t m;    
    write_bytes(MSR_CMD_MODEL, 2);
    if(read_bytes((unsigned char*)&m, sizeof(m)) != sizeof(m)) throw "Could not read model";
    if (m.msr_s != MSR_STS_MODEL_OK) {
        return (1);
    }
    return m.msr_model;    
}

unsigned char* MSR206::getFirmware()
{
    unsigned char buf[64];
    bzero (buf, sizeof(buf));
    
    write_bytes(MSR_CMD_FWREV, 2);
        
    read_bytes((unsigned char*)&buf, 4);
	if(memcmp(buf, "\x1b", 1) != 0) {
		throw "Unable to read firmware revision: Expected Escape";
	}
    if(read_bytes((unsigned char*)&buf, 5) != 5) throw "Unable to read firmware revision";
    buf[7] = '\0';
    return (unsigned char*)buf;
}

/*-------------------------------------------------------------------------------------*/
void MSR206::connect(char *devName)
{
	struct termios options;
	
	if(devName == NULL) throw "Invalid device name specified.";
	
	/* open connection to device */
	this->fd = open(devName, O_RDWR | O_NOCTTY);
	if(this->fd < 0) throw "Unable to open connection to device.";
	
	/* get options for terminal session */
	tcgetattr(this->fd, &options);
	
	/* set options */
	options.c_cflag = CS8 | CREAD | CLOCAL;
	options.c_oflag = 0;
	options.c_iflag = 0;
	
	/* set baud rate */
	cfsetispeed(&options, B9600);
	cfsetospeed(&options, B9600);
	
	/* push options */
	tcsetattr(this->fd, TCSANOW, &options);
	
}
/*-------------------------------------------------------------------------------------*/
